#include "ros/ros.h"
#include <tf/transform_listener.h>
#include <nav_msgs/Odometry.h>
#include <visualization_msgs/Marker.h>
#include <std_srvs/Empty.h>
#include "geometry.h"
#include "pid.h"

tf::Point Odom_pos;
double Odom_yaw, Odom_v, Odom_w;
ros::Publisher cmd_vel_pub, marker_pub;
ros::Subscriber odom_sub;
int num_slice = 50;
double maxSpeed = 0.5, distanceConst = 0.5;
// double dt = 0.1, maxT = M_PI, minT = -M_PI, Kp = 0.3, Ki = 0.05, Kd = 0.01;
double dt = 0.1, maxT = M_PI, minT = -M_PI, Kp = 1, Ki = 0.02, Kd = 0.1;
// double dtS = 0.1, maxS = maxSpeed, minS = 0.0, KpS = 0.08, KiS = 0.01, KdS = 0.005;
double dtS = 0.1, maxS = maxSpeed, minS = 0.0, KpS = 0.1, KiS = 0.02, KdS = 0.1;


void odomCallback(const nav_msgs::Odometry odom_msg) {
    tf::pointMsgToTF(odom_msg.pose.pose.position, Odom_pos);
    Odom_yaw = tf::getYaw(odom_msg.pose.pose.orientation);
    Odom_v = odom_msg.twist.twist.linear.x;
    Odom_w = odom_msg.twist.twist.angular.z;
}

void displayLane(bool isTrajectoryPushed, Geometry &geometry) {
    static visualization_msgs::Marker path;
    path.type = visualization_msgs::Marker::LINE_STRIP;
    path.header.frame_id = "odom";
    path.header.stamp = ros::Time::now();
    path.ns = "odom";
    path.id = 0;
    path.action = visualization_msgs::Marker::ADD;
    path.lifetime = ros::Duration();
    path.color.b = 1.0;
    path.color.a = 1.0;
    path.scale.x = 0.02;
    path.pose.orientation.w = 1.0;

    static int slice_index = 0;

    VECTOR2D *prev = nullptr, *current = nullptr;

    while (path.points.size() <= num_slice) {
        geometry_msgs::Point p;
        float angle = slice_index * 2 * M_PI / num_slice;
        slice_index++;
        p.x = 4 * cos(angle) - 0.5;
        p.y = 4 * sin(angle) + 1.0;
        p.z = 0;

        path.points.push_back(p);

        if (!isTrajectoryPushed) {
            VECTOR2D *temp = new VECTOR2D(p.x, p.y);
            geometry.trajectory.push_back(*temp);
            current = temp;

            if (prev != nullptr) {
                geometry.path.push_back(geometry.getLineSegment(*prev, *current));
            }
            prev = current;
        }
    }

    if (prev != nullptr && current != nullptr && current != prev)
        geometry.path.push_back(geometry.getLineSegment(*prev, *current));

    marker_pub.publish(path);
}

int main(int argc, char **argv) {
    ros::init(argc, argv, "control");
    ros::NodeHandle n("~");
    tf::TransformListener m_listener;
    tf::StampedTransform transform;
    cmd_vel_pub = n.advertise<geometry_msgs::Twist>("cmd_vel", 1);
    marker_pub = n.advertise<visualization_msgs::Marker>("visualization_marker", 1);
    odom_sub = n.subscribe("odom", 10, odomCallback);
    ros::Rate loop_rate(10);

    geometry_msgs::Twist tw_msg;
    Geometry geometry;

    int frame_count = 0;
    PID pidTheta(dt, maxT, minT, Kp, Kd, Ki);
    PID pidVelocity(dtS, maxS, minS, KpS, KdS, KiS);
    
    double omega = 0.0;
    while (ros::ok()) {
        if (frame_count == 0)
            displayLane(false, geometry);
        else
            displayLane(true, geometry);

        double speed = maxSpeed;
        VECTOR2D current_pos;
        current_pos.x = Odom_pos.x();
        current_pos.y = Odom_pos.y();

        Geometry::LineSegment *lineSegment = geometry.getNearestLine(current_pos);
        Geometry::LineSegment lineSegmentPerpen = geometry.getMinimumDistanceLine(*lineSegment, current_pos);
        Geometry::LineSegment *nextLinesegment = geometry.getNextLineSegment(lineSegment);

        double targetDistanceWithEnd = geometry.getDistanceSquared(current_pos, lineSegment->endP);
        double targetDistanceWithStart = geometry.getDistanceSquared(current_pos, lineSegment->startP);
        double targetAngleWithPerpen = geometry.getGradient(current_pos, lineSegment->endP);

        double angleError = 0.0;

        VECTOR2D target = lineSegment->endP;
        double targetAngle = geometry.getGradient(current_pos, target);
        double distanceToClosestPath = abs(lineSegment->disatanceToAObj);
double targetAnglePerpen = geometry.getGradient(current_pos, lineSegmentPerpen.endP);
        if (distanceToClosestPath < distanceConst) {

            double directional = targetAngle;

            double discripancy = targetAnglePerpen - directional;
            discripancy = geometry.correctAngle(discripancy);

            discripancy = 0.5* discripancy / distanceConst * abs(distanceToClosestPath);

            double combined = targetAngle + discripancy;

            angleError = combined - Odom_yaw;

        } else {
            angleError = targetAnglePerpen - Odom_yaw;
        }
        if (targetDistanceWithEnd < 0.5) {
            double futureAngleChange = nextLinesegment->gradient - lineSegment->gradient;
            futureAngleChange = geometry.correctAngle(futureAngleChange);
            futureAngleChange = futureAngleChange / distanceConst * abs(targetDistanceWithEnd);;
            double combined = targetAngle + futureAngleChange;
            angleError = combined - Odom_yaw;
        } else {
            angleError = geometry.getGradient(current_pos, lineSegmentPerpen.endP) - Odom_yaw;
        }

        double speedError = 0.0;

        if (targetDistanceWithStart < 0.7 || targetDistanceWithEnd < 0.7) {
            double targetDistance = std::min(targetDistanceWithStart, targetDistanceWithEnd);
            speedError = 0.3 * maxSpeed * exp(-abs(targetDistance));
            speed = pidVelocity.calculate(maxSpeed, -speedError);
        }

        angleError = geometry.correctAngle(angleError);
        double omega = pidTheta.calculate(0, -angleError);

        ROS_INFO("Odom_yaw %f, Angle Error: %f , omega: %f Speed %f", Odom_yaw, angleError, omega, Odom_v);

        tw_msg.linear.x = speed;
        tw_msg.angular.z = omega;
        cmd_vel_pub.publish(tw_msg);

        frame_count++;
        ros::spinOnce();
        loop_rate.sleep();
    }

    return 0;
}

